/*
 * adjust.c
 *
 *  Created on: May 22, 2021
 *      Author: kirk_
 */

#include "adjust.h"

#include "velocity.h"
#include "position.h"
#include "direction.h"

KDL_LED_t AdjustLED;

void StartAdjustThread(void *argument)
{
	uint32_t Period;
	double dtmp;

	/* Get arg */
	if(argument != NULL)
		Period = *(uint32_t*)argument;
	else
		Period = Default_Fetch_Period;

	/* Init LED */
	KDL_LED_Init(&AdjustLED, LED2_GPIO_Port, LED2_Pin, KDL_LED_ONSTATE_LOW);

	/* Init encoder */
	KDL_Encoder_Init(&Encoder[0], &htim1);
	KDL_Encoder_Init(&Encoder[1], &htim2);
	KDL_Encoder_Init(&Encoder[2], &htim8);
	KDL_Encoder_Init(&Encoder[3], &htim3);

	/* Init velocity */
	VelocityInit(NULL);
	/* Init position */
	PositionInit(NULL);
	/* Init direction */
	DirectionInit(NULL);

	/* Initialized */
	osThreadResume(defaultTaskHandle);
	/* Start timer */
	osTimerStart(AdjustTimerHandle, Period);

	/* Start encoders */
	KDL_Encoder_Start(&Encoder[0]);
	KDL_Encoder_Start(&Encoder[1]);
	KDL_Encoder_Start(&Encoder[2]);
	KDL_Encoder_Start(&Encoder[3]);

	for(;;)
	{
		/* Suspend self */
		osThreadSuspend(AdjustThreadHandle);

		/* Toggle LED */
		KDL_LED_Toggle(&AdjustLED);

		/* Fetch encoder */
		KDL_Encoder_Convert(&Encoder[0]);
		KDL_Encoder_Convert(&Encoder[1]);
		KDL_Encoder_Convert(&Encoder[2]);
		KDL_Encoder_Convert(&Encoder[3]);

		/* Adjust direction */
		KD_FloatPID_Process(&Direction, DirectionGet(), Period);

		/* Adjust position */
		dtmp = Encoder[0].Position + Encoder[3].Position;
		dtmp /= Position_Denominator;
		KD_FloatPID_Process(&PositionX, dtmp, Period);
		dtmp = Encoder[1].Position + Encoder[2].Position;
		dtmp /= Position_Denominator;
		KD_FloatPID_Process(&PositionY, dtmp, Period);

		/* Adjust velocity */
		KD_FloatPID_Process(&Velocity[0], VelocityConvert(Encoder[0].Velocity), Period);
		KD_FloatPID_Process(&Velocity[1], VelocityConvert(Encoder[1].Velocity), Period);
		KD_FloatPID_Process(&Velocity[2], VelocityConvert(Encoder[2].Velocity), Period);
		KD_FloatPID_Process(&Velocity[3], VelocityConvert(Encoder[3].Velocity), Period);
#if 0
		/* Print Error */
		PRINTF("[Velocity] Error 0 %ld;\r\n", (int32_t)(Velocity[0].Error0*1000));
		PRINTF("[Velocity] Error 1 %ld;\r\n", (int32_t)(Velocity[1].Error0*1000));
		PRINTF("[Velocity] Error 2 %ld;\r\n", (int32_t)(Velocity[2].Error0*1000));
		PRINTF("[Velocity] Error 3 %ld;\r\n", (int32_t)(Velocity[3].Error0*1000));
		PRINTF("[Position] Error X %ld;\r\n", (int32_t)(PositionX.Error0*1000));
		PRINTF("[Position] Error Y %ld;\r\n", (int32_t)(PositionY.Error0*1000));
		PRINTF("[Direction] Error %ld;\r\n", (int32_t)(Direction.Error0*1000));
#endif
	}
	/* Exit */
	osThreadExit();
	return;
}


void AdjustTimerCallback(void *argument)
{
	/* Start adjust */
	osThreadResume(AdjustThreadHandle);
}
